#!/usr/bin/env python


import roslib; roslib.load_manifest('knobs_rev')
import rospy
import message_filters
import image_geometry
import actionlib
import tf

from smach import State, StateMachine
from sensor_msgs.msg import Image, CameraInfo, PointCloud2
from arm_navigation_msgs.msg import MoveArmGoal, MoveArmAction, JointConstraint
from pr2_controllers_msgs.msg import Pr2GripperCommandGoal, Pr2GripperCommandAction

from std_msgs.msg import Float32, Header
from knobs_rev.msg import PoseArmAction, PoseArmGoal
from knobs_rev.srv import PlaneCloud, PlaneCloudRequest, PlaneCloudResponse

import cv
from cv_bridge import CvBridge, CvBridgeError
from PySide.QtCore import * 
from PySide.QtGui import * 

import tf.transformations as tt
import sys

class TurnKnob(State):
    def __init__(self):
      State.__init__(self, outcomes=['succeeded', 'failed', 'abort'], input_keys=['rotation'])
      self.right_arm_client = actionlib.SimpleActionClient('pose_arm', PoseArmAction)
      rospy.loginfo("Waiting on arm server")
      #self.right_arm_client.wait_for_server()
      rospy.loginfo("Arm server found")
      self.prev_angle = 0.0

    def execute(self, userdata):
      if rospy.is_shutdown():
         return 'abort'
      if not userdata.rotation:
         return 'failed'
      rospy.loginfo("Turning knob to {0}".format(userdata.rotation))
      #return 'succeeded'

      change = userdata.rotation - self.prev_angle
      self.prev_angle = userdata.rotation
      rospy.loginfo("Rotating by {0}".format(change))
      #self.prev_angle = userdata.rotation
      goal = PoseArmGoal()
      goal.pose.header.frame_id = 'r_wrist_roll_link'
      goal.pose.pose.position.x = 0.0
      goal.pose.pose.position.y = 0.0
      goal.pose.pose.position.z = 0.0
      temp = tt.quaternion_about_axis(change, (1,0,0)) # xaxis
      rospy.loginfo("Wrist quat: {0}".format(temp))
      goal.pose.pose.orientation.x = temp[0]
      goal.pose.pose.orientation.y = temp[1]
      goal.pose.pose.orientation.z = temp[2]
      goal.pose.pose.orientation.w = temp[3]

      self.right_arm_client.send_goal(goal)
      rospy.loginfo("Waiting for arms to clear")
      self.right_arm_client.wait_for_result()
      return 'succeeded'



class DialDisplay(State):
    def __init__(self):
        State.__init__(self, outcomes=['succeeded', 'failed', 'abort'], output_keys=['rotation'])

        self.layout = QVBoxLayout()
        self.dial = QDial()
        self.dial.valueChanged.connect(self.callback)
        self.dial.setNotchesVisible(True)
        self.dial.setWrapping(True)
        self.layout.addWidget(self.dial)

        self.button = QPushButton('done')
        self.button.clicked.connect(self.done)
        self.layout.addWidget(self.button)

        self.group = QGroupBox('knob control')
        self.group.setLayout(self.layout)

        self.pub = rospy.Publisher('knob/value', Float32)
        self.rotation = None

    def callback(self, value):
        self.pub.publish(float(value) / 99.0 * 3.14159 * 2.0)
        self.rotation = float(value) / 99.0 * 3.14159 * 2.0

    def done(self):
        self.group.hide()
        app.quit()

    def execute(self, userdata):
        rospy.loginfo("Dial display state")
        self.group.show()
        app.exec_()
        userdata.rotation = self.rotation
        return 'succeeded'


if __name__ == '__main__':
    rospy.init_node('finder')


    app = QApplication(sys.argv)
    machine = StateMachine(outcomes=['succeeded', 'failed', 'aborted'])
    with machine:
        StateMachine.add('DIAL', DialDisplay(), 
                                       transitions={'succeeded':'TURN_KNOB', 
                                                    'abort':'aborted'})
        StateMachine.add('TURN_KNOB', TurnKnob(), 
                                       transitions={'succeeded':'DIAL', 
                                                    'failed':'DIAL',
                                                    'abort':'aborted'})


    try:
      rospy.loginfo("Starting state machine execution")
      machine.execute()
      rospy.spin()
    except KeyboardInterrupt:
      rospy.loginfo("Shutting down")
    cv.DestroyAllWindows()
